#include "application.hpp"
#include <iostream>
using namespace std;
using namespace chrono;

bool Application::LoadConfig(std::string configPath)
{
    cout << "Load config file from " << configPath << endl;
    return config.LoadFile(configPath);
}

bool Application::Init()
{
    cout << "Start init devices:" << endl;
    bool hasError = false;
    state.hasStick = true;
    state.loopCount = 0;

    if (!device.imu.InitDevice(config.IMU.device, config.IMU.baudrate))
    {
        cout << "Init IMU Serial Fail" << endl;
        hasError = true;
    }

    MotorConfig leftConfig{.address = config.leftMotor.address, .poleCount = config.leftMotor.pole};
    MotorConfig rightConfig{.address = config.rightMotor.address, .poleCount = config.rightMotor.pole};

    if (config.MototrBus.type == "can")
    {
        device.useCan = true;
        auto server = make_shared<CanServer>();
        if (!server->StartServer("can0"))
        {
            cout << "Open Can Server fail." << endl;
            return false;
        }
        if (!device.motorCanLeft.InitDevice(server, leftConfig))
        {
            cout << "Init motorLeft fail" << endl;
            return false;
        }
        if (!device.motorCanRight.InitDevice(server, rightConfig))
        {
            cout << "Init motorRight fail" << endl;
            return false;
        }
    }
    else if (config.MototrBus.type == "serial")
    {
        device.useCan = false;
        modbus_t *modBusCtx =
            Motor_RS485::CreatModBusInstance(config.MototrBus.device, config.MototrBus.baudrate, false);
        if (modBusCtx == nullptr)
        {
            cout << "Creat modbus fail" << endl;
            return -1;
        }

        if (device.motorLeft.InitDevice(modBusCtx, leftConfig))
        {
            device.motorLeft.Select();
            device.motorLeft.SetControlMode(ControlMode::Speed);
        }
        else
        {
            cout << "Init left motor Fail" << endl;
            hasError = true;
        }

        if (device.motorRight.InitDevice(modBusCtx, rightConfig))
        {
            device.motorRight.Select();
            device.motorRight.SetControlMode(ControlMode::Speed);
        }
        else
        {
            cout << "Init right motor Fail" << endl;
            hasError = true;
        }
    }

    state.hasStick = device.stick.InitDevice(config.joystick.device);
    if (!state.hasStick)
    {
        cout << "Init without JoyStick" << endl;
        // hasError = true;
    }

    if (hasError)
    {
        cout << "Some error has accured,Exit" << endl;
        return -1;
    }

    // wait for hardware establish init state
    this_thread::sleep_for(chrono::milliseconds(100));
    lastTick = chrono::steady_clock::now();

    return true;
}

bool Application::Loop()
{
    // get time
    auto current = steady_clock::now();
    state.deltaTimeMs = duration_cast<microseconds>(current - lastTick).count() / 1000.0f;
    state.fps = 1000.0f / state.deltaTimeMs;
    lastTick = current;
    if (state.deltaTimeMs > 100)
    {
        state.deltaTimeMs = 0;
    }
    // get input
    float move = state.hasStick ? device.stick.GetAix(config.moveAix.number) : 0;
    move *= config.moveAix.range;
    float rotate = state.hasStick ? device.stick.GetAix(config.rotateAix.number) : 0;
    rotate *= config.rotateAix.range;

    if (!OnTick(Input{move, rotate}))
    {
        return false;
    }
    state.loopCount++;
    if (tickInterval.count() > 0)
    {
        if (chrono::steady_clock::now() - current > tickInterval)
        {
            cout << "Warning Time tick overlap" << endl;
        }
        steady_clock::time_point tickEnd = current + tickInterval;
        this_thread::sleep_until(tickEnd);
    }
    return true;
}

void Application::SetTickInterval(float ms)
{
    tickInterval = chrono::duration_cast<chrono::milliseconds>(chrono::duration<float, milli>(ms));
}

void Application::FrequencyDividerInvoke(int divid, int &count, const std::function<void()> &func)
{
    count++;
    if (count >= divid)
    {
        func();
        count = 0;
    }
}

void Application::PrintOut(Message message)
{
    for (auto pair : message)
    {
        cout.precision(3);
        cout.width(10);
        cout << pair.first << ':' << pair.second << '\t';
    }
    cout << endl;
}

void Application::RecordAll(Recorder &recorder, Message message)
{
    if (recorder.Lines() == 0)
    {
        for (auto pair : message)
        {
            recorder.SaveString(pair.first);
        }
        recorder.NewLine();
    }
    for (auto pair : message)
    {
        recorder.SaveFloat(pair.second);
    }
    recorder.NewLine();
}

void Application::ServerUpload(NetServer &server, Message message)
{
    for (auto pair : message)
    {
        server.SetValue(pair.first, pair.second);
    }
}

bool Application::CheckValueRange(float value, float rangeMin, float rangeMax, std::string tag)
{
    if (value < rangeMin || value > rangeMax)
    {
        cout << tag << " overlap [" << value << "], Exit" << endl;
        return false;
    }
    return true;
}
void Application::CheckValueRangeAndExit(float value, float rangeMin, float rangeMax, std::string tag)
{
    if (!CheckValueRange(value, rangeMin, rangeMax, tag))
    {
        exit(-1);
    }
}

void Application::SimpleStart(std::string configPath)
{
    if (!LoadConfig(configPath))
    {
        throw "Config load fail";
    }
    if (!Init())
    {
        throw "App init fail";
    }
    while (true)
    {
        if (!Loop())
        {
            break;
        }
    };

    Quit();
    std::cout << "Application Quit" << std::endl;
}